﻿using System;
using System.Collections.Generic;
using System.Collections;
using System.Text;
using Geometry;

namespace PotentialFieldDriver
{
    class PathOptimizer
    {
        public static bool isPossible(List<Point2D> path, ArrayList walls)
        {
            for (int pointNo = 0; pointNo < path.Count - 1; pointNo++)
            {
                foreach (Wall2D wall in walls)
                {
                    if (CollisionChecker.checkCollicionSection(path[pointNo], path[pointNo], wall.firstPoint, wall.secondPoint))
                    {
                        return false;
                    }
                }
            }
            return true;
        }

        public static double getAngle(Point2D a1, Point2D a2, Point2D a3)
        {
            Vector2D v1 = new Vector2D(a1, a2);
            Vector2D v2 = new Vector2D(a2, a3);
            double angle = v1.AngleBetween(v2);
            if (angle < 0)
            {
                angle = 0 - angle;
            }
            return angle;
        }

        private static bool addMidlePoints(List<Point2D> path, double pointDist, double closeDist)
        {
            for (int pointNo = 0; pointNo < path.Count - 1; pointNo++)
            {
                if (path[pointNo].GetDistance(path[pointNo + 1]) > pointDist)
                {
                    Point2D extraPoint = new Point2D((path[pointNo].x + path[pointNo + 1].x) / 2, (path[pointNo].y + path[pointNo + 1].y) / 2);
                    path.Insert(pointNo + 1, extraPoint);
                    return true;
                }
                if (path[pointNo].GetDistance(path[pointNo + 1]) < closeDist && pointNo != path.Count - 1)
                {
                    path.RemoveAt(pointNo + 1);
                    return true;
                }
            }
            return false;
        }

        private static Point2D moveIn(List<Point2D> pathFragment, int pointNo)
        {
            Point2D middle = new Point2D((pathFragment[pointNo - 1].x + pathFragment[pointNo + 1].x) / 2, (pathFragment[pointNo - 1].y + pathFragment[pointNo + 1].y) / 2);
            double movex = (middle.x - pathFragment[pointNo].x) * 0.01;
            double movey = (middle.y - pathFragment[pointNo].y) * 0.01;
            Point2D newPoint = new Point2D(pathFragment[pointNo].x + movex, pathFragment[pointNo].y + movey);
            return newPoint;
        }

        private static Point2D moveOut(List<Point2D> pathFragment, int pointNo)
        {
            Point2D middle = new Point2D((pathFragment[pointNo - 1].x + pathFragment[pointNo + 1].x) / 2, (pathFragment[pointNo - 1].y + pathFragment[pointNo + 1].y) / 2);
            double movex = (middle.x - pathFragment[pointNo].x) * 0.01;
            double movey = (middle.y - pathFragment[pointNo].y) * 0.01;
            Point2D newPoint = new Point2D(pathFragment[pointNo].x - movex, pathFragment[pointNo].y - movey);
            return newPoint;
        }


        private static bool checkCollisionInPartPath(List<Point2D> path, ArrayList walls)
        {
            foreach(Wall2D wall in walls) {
                for(int i=0;i<path.Count-2;i++) {
                    if (CollisionChecker.checkCollicionSection(path[i], path[i + 1], wall.firstPoint, wall.secondPoint))
                    {
                        return true;
                    }
                }
            }
            return false;
        }


        private static bool optimizeAngles(List<Point2D> path, ArrayList walls)
        {
            bool optimalizationDone = false;
                for (int pointNo = 2; pointNo < path.Count - 2; pointNo++)
                {
                    double angleP = getAngle(path[pointNo - 2], path[pointNo - 1], path[pointNo]);
                    double angleC = getAngle(path[pointNo - 1], path[pointNo], path[pointNo + 1]);
                    double angleN = getAngle(path[pointNo], path[pointNo + 1], path[pointNo + 2]);
                    if (angleC > angleP && angleC > angleN)
                    {
                        Point2D oldPoint = path[pointNo];
                        Point2D newIn = moveIn(path, pointNo);
                        path.RemoveAt(pointNo);
                        path.Insert(pointNo, newIn);
                        if (checkCollisionInPartPath(path, walls))
                        {
                            path.RemoveAt(pointNo);
                            path.Insert(pointNo, oldPoint);
                            if (pointNo > 2 && pointNo < path.Count - 3)
                            {
                                Point2D oldPointN = path[pointNo + 1];
                                path.RemoveAt(pointNo + 1);
                                path.Insert(pointNo + 1, moveOut(path, pointNo + 1));

                                Point2D oldPointP = path[pointNo - 1];
                                path.RemoveAt(pointNo - 1);
                                path.Insert(pointNo - 1, moveOut(path, pointNo - 1));
                                if (checkCollisionInPartPath(path, walls))
                                {
                                    path.RemoveAt(pointNo + 1);
                                    path.Insert(pointNo + 1, oldPointN);

                                    path.RemoveAt(pointNo - 1);
                                    path.Insert(pointNo - 1, oldPointP);
                                }
                            }
                        }
                        else
                        {
                            optimalizationDone = true;
                        }
                    }
                }
            return optimalizationDone;
        }


        public static List<Point2D> optimize(List<Point2D> oldPath, ArrayList walls, double pointDist, double closeDist)
        {
            ArrayList WallsRect = CollisionChecker.getRectWalls(walls);
            List<Point2D> path = new List<Point2D>(oldPath);
            if (!path[0].Equals(path[path.Count - 1]))
            {
                path.Add(path[0]);
            }
            bool optimizationDone = true;
            int optimalizationsLeft = 300;
            while (optimizationDone && optimalizationsLeft>0)
            {
                optimizationDone = addMidlePoints(path, pointDist, closeDist);
                optimizationDone |= optimizeAngles(path, WallsRect);
                optimalizationsLeft--;
            }
            path.RemoveAt(path.Count - 1);
            return path;
        }
    }
}
